// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ExternalDatasetGroundTruthFileReader.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Threading;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// The file reader that can read the external dataset ground truth and can publish it as ground truth messages
    /// </summary>
    public class ExternalDatasetGroundTruthFileReader : IFileReader, IDisposable
    {
        /// <summary>
        /// The file to open and read
        /// </summary>
        private string filePath;

        /// <summary>
        /// The queue of deserialized messages waiting to be published at the right time
        /// </summary>
        private BlockingCollection<AgentMessage> publishingQueue;

        /// <summary>
        /// A cancellation token for the publishing queue
        /// </summary>
        private CancellationTokenSource cancellationTokenSource;

        /// <summary>
        /// The next available ground truth id
        /// </summary>
        private long nextId;

        /// <summary>
        /// The delta between message timestamps in the ground truth file and the timestamps in the sensor file, if any 
        /// Some curated sensor files use the relative timestamp. The original sensor file has both the relative and the absolute timestamp.
        /// The ground truth file has just the absolute timestamp. To solve this, we add (offline) a line prefixed with #start 
        /// to the ground truth file, relating the relative time to the absolute time.
        /// </summary>
        private long timeOffset;

        /// <summary>
        /// The set of id and time pairs already published. These might be referenced by subsequent messages (as the start of the ground truth segment)
        /// </summary>
        private Dictionary<long, long> publishedTimes;

        /// <summary>
        /// Initializes a new instance of the ExternalDatasetSensorFileReader class
        /// </summary>
        /// <param name="filePath">The file path</param>
        /// <param name="publishingQueue">The queue where to store the messages deserialized so far</param>
        /// <returns>The recording start time, if persisted in the file</returns>
        public long Initialize(string filePath, BlockingCollection<AgentMessage> publishingQueue)
        {
            this.publishingQueue = publishingQueue;
            this.filePath = filePath;

            // first line in the ground truth file should have the start time of the recording
            string line = null;
            using (StreamReader fileReader = new StreamReader(this.filePath))
            {
                line = fileReader.ReadLine();
            }

            string[] parts = line.Split();
            if (parts[0] != "#start")
            {
                throw new Exception("Ground truth file is missing the start time entry. Please add one.\n\r" + this.filePath);
            }

            this.timeOffset = (long)(double.Parse(parts[1]) * 1000);
            return 0;
        }

        /// <summary>
        /// Deserializes messages and inserts them in the publishing queue.
        /// This method blocks until Stop is called or until the end of file is reached.
        /// </summary>
        public void Start()
        {
            this.cancellationTokenSource = new CancellationTokenSource();
            CancellationToken cancellationToken = this.cancellationTokenSource.Token;

            this.publishedTimes = new Dictionary<long, long>();

            using (StreamReader fileReader = new StreamReader(this.filePath))
            {
                // loop over all entries in the file and add them to the queue.
                // the queue will block if we are going too fast.
                try
                {
                    while (!fileReader.EndOfStream)
                    {
                        string line = fileReader.ReadLine();
                        if (string.IsNullOrEmpty(line) || line[0] == '#')
                        {
                            continue;
                        }

                        string[] parts = line.Split();
                        long originatingStartTime = (long)(double.Parse(parts[0]) * 1000) - this.timeOffset;
                        long originatingEndTime = (long)(double.Parse(parts[1]) * 1000) - this.timeOffset;
                        if (!this.publishedTimes.ContainsKey(originatingStartTime))
                        {
                            // post a message representing the start time. 
                            this.AddGroundTruthMessage(new Pose2D(), 0, originatingStartTime, cancellationToken);
                        }

                        // post a message representing the end time. 
                        double x = double.Parse(parts[2]);
                        double y = double.Parse(parts[3]);
                        double heading = double.Parse(parts[7]);
                        this.AddGroundTruthMessage(new Pose2D(x, y, heading), this.publishedTimes[originatingStartTime], originatingEndTime, cancellationToken);
                    }
                }
                catch (OperationCanceledException)
                {
                    // Do nothing, the agent cancelled the operation
                }

                this.publishingQueue.CompleteAdding();
            }
        }

        /// <summary>
        /// Stops listening for messages
        /// </summary>
        public void Stop()
        {
            this.cancellationTokenSource.Cancel();
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="ExternalDatasetGroundTruthFileReader"/> class.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
        }

        /// <summary>
        /// Creates a new ground truth message and adds it to the publishing queue and to the dictionary of ids
        /// </summary>
        /// <param name="pose">The relative pose of the robot</param>
        /// <param name="relativeId">The id of the previous pose this message is relative to</param>
        /// <param name="originatingTime">The timestamp of the pose</param>
        /// <param name="cancellationToken">The cancellation token</param>
        private void AddGroundTruthMessage(Pose2D pose, long relativeId, long originatingTime, CancellationToken cancellationToken)
        {
            this.nextId++;
            GroundTruthMessage msg = new GroundTruthMessage(this.nextId, relativeId, pose, originatingTime);
            this.publishedTimes[originatingTime] = this.nextId;
            this.publishingQueue.TryAdd(msg, -1, cancellationToken);
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="ExternalDatasetGroundTruthFileReader"/> class.
        /// </summary>
        /// <param name="disposing">True if disposing, false if finalizing.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                if (this.cancellationTokenSource != null)
                {
                    this.cancellationTokenSource.Cancel();
                    this.cancellationTokenSource.Dispose();
                }

                GC.SuppressFinalize(this);
            }
        }
    }
}
